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_CONTACTS_CONTACT_6D_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
12
13			#include "crocoddyl/core/utils/deprecate.hpp"
14			#include "crocoddyl/multibody/contact-base.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16
17			namespace crocoddyl {
18
19			template <typename _Scalar>
20			class ContactModel6DTpl : public ContactModelAbstractTpl<_Scalar> {
21			public:
22			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23		✗	CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel6DTpl)
24
25			typedef _Scalar Scalar;
26			typedef MathBaseTpl<Scalar> MathBase;
27			typedef ContactModelAbstractTpl<Scalar> Base;
28			typedef ContactData6DTpl<Scalar> Data;
29			typedef StateMultibodyTpl<Scalar> StateMultibody;
30			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
31			typedef pinocchio::SE3Tpl<Scalar> SE3;
32			typedef typename MathBase::Vector2s Vector2s;
33			typedef typename MathBase::Vector3s Vector3s;
34			typedef typename MathBase::VectorXs VectorXs;
35			typedef typename MathBase::Matrix3s Matrix3s;
36
37			/**
38			* @brief Initialize the 6d contact model
39			*
40			* To learn more about the computation of the contact derivatives in different
41			* frames see
42			* S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
43			* Frames: Application to Polishing with Talos, ICHR 2022
44			*
45			* @param[in] state State of the multibody system
46			* @param[in] id Reference frame id of the contact
47			* @param[in] pref Contact placement used for the Baumgarte stabilization
48			* @param[in] type Type of contact
49			* @param[in] nu Dimension of the control vector
50			* @param[in] gains Baumgarte stabilization gains
51			*/
52			ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
53			const pinocchio::FrameIndex id, const SE3& pref,
54			const pinocchio::ReferenceFrame type, const std::size_t nu,
55			const Vector2s& gains = Vector2s::Zero());
56
57			/**
58			* @brief Initialize the 6d contact model
59			*
60			* The default `nu` is obtained from `StateAbstractTpl::get_nv()`. To learn
61			* more about the computation of the contact derivatives in different frames
62			* see
63			* S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
64			* Frames: Application to Polishing with Talos, ICHR 2022
65			*
66			* @param[in] state State of the multibody system
67			* @param[in] id Reference frame id of the contact
68			* @param[in] pref Contact placement used for the Baumgarte stabilization
69			* @param[in] type Type of contact
70			* @param[in] gains Baumgarte stabilization gains
71			*/
72			ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
73			const pinocchio::FrameIndex id, const SE3& pref,
74			const pinocchio::ReferenceFrame type,
75			const Vector2s& gains = Vector2s::Zero());
76
77			DEPRECATED(
78			"Use constructor that passes the type type of contact, this assumes is "
79			"pinocchio::LOCAL",
80			ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
81			const pinocchio::FrameIndex id, const SE3& pref,
82			const std::size_t nu,
83			const Vector2s& gains = Vector2s::Zero());)
84			DEPRECATED(
85			"Use constructor that passes the type type of contact, this assumes is "
86			"pinocchio::LOCAL",
87			ContactModel6DTpl(std::shared_ptr<StateMultibody> state,
88			const pinocchio::FrameIndex id, const SE3& pref,
89			const Vector2s& gains = Vector2s::Zero());)
90		✗	virtual ~ContactModel6DTpl() = default;
91
92			/**
93			* @brief Compute the 3d contact Jacobian and drift
94			*
95			* @param[in] data 3d contact data
96			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
97			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
98			*/
99			virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
100			const Eigen::Ref<const VectorXs>& x) override;
101
102			/**
103			* @brief Compute the derivatives of the 6d contact holonomic constraint
104			*
105			* @param[in] data 6d contact data
106			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
107			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
108			*/
109			virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
110			const Eigen::Ref<const VectorXs>& x) override;
111
112			/**
113			* @brief Convert the force into a stack of spatial forces
114			*
115			* @param[in] data 6d contact data
116			* @param[in] force 6d force
117			*/
118			virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
119			const VectorXs& force) override;
120
121			/**
122			* @brief Create the 6d contact data
123			*/
124			virtual std::shared_ptr<ContactDataAbstract> createData(
125			pinocchio::DataTpl<Scalar>* const data) override;
126
127			/**
128			* @brief Cast the contact-6d model to a different scalar type.
129			*
130			* It is useful for operations requiring different precision or scalar types.
131			*
132			* @tparam NewScalar The new scalar type to cast to.
133			* @return ContactModel6DTpl<NewScalar> A contact model with the
134			* new scalar type.
135			*/
136			template <typename NewScalar>
137			ContactModel6DTpl<NewScalar> cast() const;
138
139			/**
140			* @brief Return the reference frame placement
141			*/
142			const SE3& get_reference() const;
143
144			/**
145			* @brief Return the Baumgarte stabilization gains
146			*/
147			const Vector2s& get_gains() const;
148
149			/**
150			* @brief Modify the reference frame placement
151			*/
152			void set_reference(const SE3& reference);
153
154			/**
155			* @brief Print relevant information of the 6d contact model
156			*
157			* @param[out] os Output stream object
158			*/
159			virtual void print(std::ostream& os) const override;
160
161			protected:
162			using Base::id_;
163			using Base::nc_;
164			using Base::nu_;
165			using Base::state_;
166			using Base::type_;
167
168			private:
169			SE3 pref_; //!< Contact placement used for the Baumgarte stabilization
170			Vector2s gains_; //!< Baumgarte stabilization gains
171			};
172
173			template <typename _Scalar>
174			struct ContactData6DTpl : public ContactDataAbstractTpl<_Scalar> {
175			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
176
177			typedef _Scalar Scalar;
178			typedef MathBaseTpl<Scalar> MathBase;
179			typedef ContactDataAbstractTpl<Scalar> Base;
180			typedef typename MathBase::Matrix3s Matrix3s;
181			typedef typename MathBase::Matrix6xs Matrix6xs;
182			typedef typename MathBase::Matrix6s Matrix6s;
183			typedef typename MathBase::MatrixXs MatrixXs;
184			typedef typename pinocchio::SE3Tpl<Scalar> SE3;
185			typedef typename pinocchio::MotionTpl<Scalar> Motion;
186			typedef typename pinocchio::ForceTpl<Scalar> Force;
187
188			template <template <typename Scalar> class Model>
189		✗	ContactData6DTpl(Model<Scalar>* const model,
190			pinocchio::DataTpl<Scalar>* const data)
191			: Base(model, data),
192		✗	rMf(SE3::Identity()),
193		✗	lwaMl(SE3::Identity()),
194		✗	v(Motion::Zero()),
195		✗	a0_local(Motion::Zero()),
196		✗	f_local(Force::Zero()),
197		✗	da0_local_dx(6, model->get_state()->get_ndx()),
198		✗	fJf(6, model->get_state()->get_nv()),
199		✗	v_partial_dq(6, model->get_state()->get_nv()),
200		✗	a_partial_dq(6, model->get_state()->get_nv()),
201		✗	a_partial_dv(6, model->get_state()->get_nv()),
202		✗	a_partial_da(6, model->get_state()->get_nv()),
203		✗	fJf_df(6, model->get_state()->get_nv()) {
204		✗	frame = model->get_id();
205		✗	jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
206		✗	fXj = jMf.inverse().toActionMatrix();
207		✗	da0_local_dx.setZero();
208		✗	fJf.setZero();
209		✗	v_partial_dq.setZero();
210		✗	a_partial_dq.setZero();
211		✗	a_partial_dv.setZero();
212		✗	a_partial_da.setZero();
213		✗	av_world_skew.setZero();
214		✗	aw_world_skew.setZero();
215		✗	av_skew.setZero();
216		✗	aw_skew.setZero();
217		✗	fv_skew.setZero();
218		✗	fw_skew.setZero();
219		✗	rMf_Jlog6.setZero();
220		✗	fJf_df.setZero();
221		✗	}
222		✗	virtual ~ContactData6DTpl() = default;
223
224			using Base::a0;
225			using Base::da0_dx;
226			using Base::df_du;
227			using Base::df_dx;
228			using Base::f;
229			using Base::frame;
230			using Base::fXj;
231			using Base::Jc;
232			using Base::jMf;
233			using Base::pinocchio;
234
235			SE3 rMf;
236			SE3 lwaMl;
237			Motion v;
238			Motion a0_local;
239			Force f_local;
240			Matrix6xs da0_local_dx;
241			MatrixXs fJf;
242			Matrix6xs v_partial_dq;
243			Matrix6xs a_partial_dq;
244			Matrix6xs a_partial_dv;
245			Matrix6xs a_partial_da;
246			Matrix3s av_world_skew;
247			Matrix3s aw_world_skew;
248			Matrix3s av_skew;
249			Matrix3s aw_skew;
250			Matrix3s fv_skew;
251			Matrix3s fw_skew;
252			Matrix6s rMf_Jlog6;
253			MatrixXs fJf_df;
254			};
255
256			} // namespace crocoddyl
257			/* --- Details -------------------------------------------------------------- */
258			/* --- Details -------------------------------------------------------------- */
259			/* --- Details -------------------------------------------------------------- */
260			#include "crocoddyl/multibody/contacts/contact-6d.hxx"
261
262			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel6DTpl)
263			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData6DTpl)
264
265			#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_6D_HPP_
266