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