GCC Code Coverage Report

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