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			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
11			#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
12
13			#include "crocoddyl/core/residual-base.hpp"
14			#include "crocoddyl/multibody/contact-base.hpp"
15			#include "crocoddyl/multibody/contacts/contact-2d.hpp"
16			#include "crocoddyl/multibody/contacts/contact-3d.hpp"
17			#include "crocoddyl/multibody/contacts/contact-6d.hpp"
18			#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
19			#include "crocoddyl/multibody/data/contacts.hpp"
20			#include "crocoddyl/multibody/data/impulses.hpp"
21			#include "crocoddyl/multibody/friction-cone.hpp"
22			#include "crocoddyl/multibody/fwd.hpp"
23			#include "crocoddyl/multibody/impulse-base.hpp"
24			#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
25			#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
26			#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
27			#include "crocoddyl/multibody/states/multibody.hpp"
28
29			namespace crocoddyl {
30
31			/**
32			* @brief Contact friction cone residual
33			*
34			* This residual function is defined as
35			* \f$\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\f$, where
36			* \f$\mathbf{A}\in~\mathbb{R}^{nr\times nc}\f$ describes the linearized
37			* friction cone, \f$\boldsymbol{\lambda}\in~\mathbb{R}^{nc}\f$ is the spatial
38			* contact forces computed by `DifferentialActionModelContactFwdDynamicsTpl`,
39			* and `nr`, `nc` are the number of cone facets and dimension of the contact,
40			* respectively.
41			*
42			* Both residual and residual Jacobians are computed analytically, where th
43			* force vector \f$\boldsymbol{\lambda}\f$ and its Jacobians
44			* \f$\left(\frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{x}},
45			* \frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{u}}\right)\f$ are
46			* computed by `DifferentialActionModelContactFwdDynamicsTpl` or
47			* `ActionModelImpulseFwdDynamicTpl`. These values are stored in a shared data
48			* (i.e. `DataCollectorContactTpl` or `DataCollectorImpulseTpl`). Note that
49			* this residual function cannot be used with other action models.
50			*
51			* As described in `ResidualModelAbstractTpl()`, the residual value and its
52			* derivatives are calculated by `calc` and `calcDiff`, respectively.
53			*
54			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`,
55			* `DifferentialActionModelContactFwdDynamicsTpl`,
56			* `ActionModelImpulseFwdDynamicTpl`, `DataCollectorForceTpl`
57			*/
58			template <typename _Scalar>
59			class ResidualModelContactFrictionConeTpl
60			: public ResidualModelAbstractTpl<_Scalar> {
61			public:
62			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63		✗	CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelContactFrictionConeTpl)
64
65			typedef _Scalar Scalar;
66			typedef MathBaseTpl<Scalar> MathBase;
67			typedef ResidualModelAbstractTpl<Scalar> Base;
68			typedef ResidualDataContactFrictionConeTpl<Scalar> Data;
69			typedef StateMultibodyTpl<Scalar> StateMultibody;
70			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
71			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
72			typedef FrictionConeTpl<Scalar> FrictionCone;
73			typedef typename MathBase::VectorXs VectorXs;
74			typedef typename MathBase::MatrixXs MatrixXs;
75			typedef typename MathBase::MatrixX3s MatrixX3s;
76
77			/**
78			* @brief Initialize the contact friction cone residual model
79			*
80			* Note that for the inverse-dynamic cases, the control vector contains the
81			* generalized accelerations, torques, and all the contact forces.
82			*
83			* @param[in] state State of the multibody system
84			* @param[in] id Reference frame id
85			* @param[in] fref Reference friction cone
86			* @param[in] nu Dimension of the control vector
87			* @param[in] fwddyn Indicates that we have a forward dynamics problem (true)
88			* or inverse dynamics (false)
89			*/
90			ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
91			const pinocchio::FrameIndex id,
92			const FrictionCone& fref,
93			const std::size_t nu,
94			const bool fwddyn = true);
95
96			/**
97			* @brief Initialize the contact friction cone residual model
98			*
99			* The default `nu` value is obtained from `StateAbstractTpl::get_nv()`. Note
100			* that this constructor can be used for forward-dynamics cases only.
101			*
102			* @param[in] state State of the multibody system
103			* @param[in] id Reference frame id
104			* @param[in] fref Reference friction cone
105			*/
106			ResidualModelContactFrictionConeTpl(std::shared_ptr<StateMultibody> state,
107			const pinocchio::FrameIndex id,
108			const FrictionCone& fref);
109		✗	virtual ~ResidualModelContactFrictionConeTpl() = default;
110
111			/**
112			* @brief Compute the contact friction cone residual
113			*
114			* @param[in] data Contact friction cone residual data
115			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
116			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
117			*/
118			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
119			const Eigen::Ref<const VectorXs>& x,
120			const Eigen::Ref<const VectorXs>& u) override;
121
122			/**
123			* @brief Compute the residual vector for nodes that depends only on the state
124			*
125			* It updates the residual vector based on the state only (i.e., it ignores
126			* the contact forces). This function is used in the terminal nodes of an
127			* optimal control problem.
128			*
129			* @param[in] data Residual data
130			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
131			*/
132			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
133			const Eigen::Ref<const VectorXs>& x) override;
134
135			/**
136			* @brief Compute the Jacobians of the contact friction cone residual
137			*
138			* @param[in] data Contact friction cone residual data
139			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
140			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
141			*/
142			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
143			const Eigen::Ref<const VectorXs>& x,
144			const Eigen::Ref<const VectorXs>& u) override;
145
146			/**
147			* @brief Compute the Jacobian of the residual functions with respect to the
148			* state only
149			*
150			* It updates the Jacobian of the residual function based on the state only
151			* (i.e., it ignores the contact forces). This function is used in the
152			* terminal nodes of an optimal control problem.
153			*
154			* @param[in] data Residual data
155			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
156			*/
157			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
158			const Eigen::Ref<const VectorXs>& x) override;
159
160			/**
161			* @brief Create the contact friction cone residual data
162			*/
163			virtual std::shared_ptr<ResidualDataAbstract> createData(
164			DataCollectorAbstract* const data) override;
165
166			/**
167			* @brief Update the Jacobians of the contact friction cone residual
168			*
169			* @param[in] data Contact friction cone residual data
170			*/
171			void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);
172
173			/**
174			* @brief Cast the contact-friction-cone residual model to a different scalar
175			* type.
176			*
177			* It is useful for operations requiring different precision or scalar types.
178			*
179			* @tparam NewScalar The new scalar type to cast to.
180			* @return ResidualModelContactFrictionConeTpl<NewScalar> A residual model
181			* with the new scalar type.
182			*/
183			template <typename NewScalar>
184			ResidualModelContactFrictionConeTpl<NewScalar> cast() const;
185
186			/**
187			* @brief Indicates if we are using the forward-dynamics (true) or
188			* inverse-dynamics (false)
189			*/
190			bool is_fwddyn() const;
191
192			/**
193			* @brief Return the reference frame id
194			*/
195			pinocchio::FrameIndex get_id() const;
196
197			/**
198			* @brief Return the reference contact friction cone
199			*/
200			const FrictionCone& get_reference() const;
201
202			/**
203			* @brief Modify the reference frame id
204			*/
205			DEPRECATED("Do not use set_id, instead create a new model",
206			void set_id(const pinocchio::FrameIndex id);)
207
208			/**
209			* @brief Modify the reference contact friction cone
210			*/
211			void set_reference(const FrictionCone& reference);
212
213			/**
214			* @brief Print relevant information of the contact-friction-cone residual
215			*
216			* @param[out] os Output stream object
217			*/
218			virtual void print(std::ostream& os) const override;
219
220			protected:
221			using Base::nu_;
222			using Base::state_;
223
224			private:
225			bool fwddyn_; //!< Indicates if we are using this function for forward
226			//!< dynamics
227			bool update_jacobians_; //!< Indicates if we need to update the Jacobians
228			//!< (used for inverse dynamics case)
229			pinocchio::FrameIndex id_; //!< Reference frame id
230			FrictionCone fref_; //!< Reference contact friction cone
231			};
232
233			template <typename _Scalar>
234			struct ResidualDataContactFrictionConeTpl
235			: public ResidualDataAbstractTpl<_Scalar> {
236			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
237
238			typedef _Scalar Scalar;
239			typedef MathBaseTpl<Scalar> MathBase;
240			typedef ResidualDataAbstractTpl<Scalar> Base;
241			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
242			typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
243			typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
244			typedef StateMultibodyTpl<Scalar> StateMultibody;
245			typedef typename MathBase::MatrixXs MatrixXs;
246
247			template <template <typename Scalar> class Model>
248		✗	ResidualDataContactFrictionConeTpl(Model<Scalar>* const model,
249			DataCollectorAbstract* const data)
250		✗	: Base(model, data) {
251		✗	contact_type = ContactUndefined;
252			// Check that proper shared data has been passed
253		✗	bool is_contact = true;
254		✗	DataCollectorContactTpl<Scalar>* d1 =
255		✗	dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
256		✗	DataCollectorImpulseTpl<Scalar>* d2 =
257		✗	dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
258		✗	if (d1 == NULL && d2 == NULL) {
259		✗	throw_pretty(
260			"Invalid argument: the shared data should be derived from "
261			"DataCollectorContact or DataCollectorImpulse");
262			}
263		✗	if (d2 != NULL) {
264		✗	is_contact = false;
265			}
266
267			// Avoids data casting at runtime
268		✗	const pinocchio::FrameIndex id = model->get_id();
269		✗	const std::shared_ptr<StateMultibody>& state =
270			std::static_pointer_cast<StateMultibody>(model->get_state());
271		✗	std::string frame_name = state->get_pinocchio()->frames[id].name;
272		✗	bool found_contact = false;
273		✗	if (is_contact) {
274		✗	for (typename ContactModelMultiple::ContactDataContainer::iterator it =
275		✗	d1->contacts->contacts.begin();
276		✗	it != d1->contacts->contacts.end(); ++it) {
277		✗	if (it->second->frame == id) {
278		✗	ContactData2DTpl<Scalar>* d2d =
279		✗	dynamic_cast<ContactData2DTpl<Scalar>*>(it->second.get());
280		✗	if (d2d != NULL) {
281		✗	contact_type = Contact2D;
282		✗	found_contact = true;
283		✗	contact = it->second;
284		✗	break;
285			}
286		✗	ContactData3DTpl<Scalar>* d3d =
287		✗	dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
288		✗	if (d3d != NULL) {
289		✗	contact_type = Contact3D;
290		✗	found_contact = true;
291		✗	contact = it->second;
292		✗	break;
293			}
294		✗	ContactData6DTpl<Scalar>* d6d =
295		✗	dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
296		✗	if (d6d != NULL) {
297		✗	contact_type = Contact6D;
298		✗	found_contact = true;
299		✗	contact = it->second;
300		✗	break;
301			}
302		✗	throw_pretty(
303			"Domain error: there isn't defined at least a 2d contact for " +
304			frame_name);
305			break;
306			}
307			}
308			} else {
309		✗	for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
310		✗	d2->impulses->impulses.begin();
311		✗	it != d2->impulses->impulses.end(); ++it) {
312		✗	if (it->second->frame == id) {
313		✗	ImpulseData3DTpl<Scalar>* d3d =
314		✗	dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
315		✗	if (d3d != NULL) {
316		✗	contact_type = Contact3D;
317		✗	found_contact = true;
318		✗	contact = it->second;
319		✗	break;
320			}
321		✗	ImpulseData6DTpl<Scalar>* d6d =
322		✗	dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
323		✗	if (d6d != NULL) {
324		✗	contact_type = Contact6D;
325		✗	found_contact = true;
326		✗	contact = it->second;
327		✗	break;
328			}
329		✗	throw_pretty(
330			"Domain error: there isn't defined at least a 3d contact for " +
331			frame_name);
332			break;
333			}
334			}
335			}
336		✗	if (!found_contact) {
337		✗	throw_pretty("Domain error: there isn't defined contact data for " +
338			frame_name);
339			}
340		✗	}
341		✗	virtual ~ResidualDataContactFrictionConeTpl() = default;
342
343			std::shared_ptr<ForceDataAbstractTpl<Scalar> >
344			contact; //!< Contact force data
345			ContactType contact_type; //!< Type of contact (2D / 3D / 6D)
346			using Base::r;
347			using Base::Ru;
348			using Base::Rx;
349			using Base::shared;
350			};
351
352			} // namespace crocoddyl
353
354			/* --- Details -------------------------------------------------------------- */
355			/* --- Details -------------------------------------------------------------- */
356			/* --- Details -------------------------------------------------------------- */
357			#include "crocoddyl/multibody/residuals/contact-friction-cone.hxx"
358
359			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
360			crocoddyl::ResidualModelContactFrictionConeTpl)
361			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
362			crocoddyl::ResidualDataContactFrictionConeTpl)
363
364			#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FRICTION_CONE_HPP_
365