GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2020-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_1D_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_
12
13			#include "crocoddyl/multibody/contact-base.hpp"
14			#include "crocoddyl/multibody/fwd.hpp"
15
16			namespace crocoddyl {
17
18			template <typename _Scalar>
19			class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
20			public:
21			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22		✗	CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel1DTpl)
23
24			typedef _Scalar Scalar;
25			typedef MathBaseTpl<Scalar> MathBase;
26			typedef ContactModelAbstractTpl<Scalar> Base;
27			typedef ContactData1DTpl<Scalar> Data;
28			typedef StateMultibodyTpl<Scalar> StateMultibody;
29			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
30			typedef typename MathBase::Vector2s Vector2s;
31			typedef typename MathBase::Vector3s Vector3s;
32			typedef typename MathBase::VectorXs VectorXs;
33			typedef typename MathBase::Matrix3s Matrix3s;
34
35			/**
36			* @brief Initialize the 1d contact model
37			*
38			* To learn more about the computation of the contact derivatives in different
39			* frames see
40			* S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary
41			* Frames: Application to Polishing with Talos, ICHR 2022
42			*
43			* @param[in] state State of the multibody system
44			* @param[in] id Reference frame id of the contact
45			* @param[in] xref Contact position used for the Baumgarte stabilization
46			* @param[in] type Type of contact
47			* @param[in] rotation Rotation of the reference frame's z-axis
48			* @param[in] nu Dimension of the control vector
49			* @param[in] gains Baumgarte stabilization gains
50			*/
51			ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
52			const pinocchio::FrameIndex id, const Scalar xref,
53			const pinocchio::ReferenceFrame type,
54			const Matrix3s& rotation, const std::size_t nu,
55			const Vector2s& gains = Vector2s::Zero());
56
57			/**
58			* @brief Initialize the 1d 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] xref Contact position used for the Baumgarte stabilization
69			* @param[in] type Type of contact
70			* @param[in] gains Baumgarte stabilization gains
71			*/
72			ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
73			const pinocchio::FrameIndex id, const Scalar xref,
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			ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
81			const pinocchio::FrameIndex id, const Scalar xref,
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			ContactModel1DTpl(std::shared_ptr<StateMultibody> state,
88			const pinocchio::FrameIndex id, const Scalar xref,
89			const Vector2s& gains = Vector2s::Zero());)
90		✗	virtual ~ContactModel1DTpl() = default;
91
92			/**
93			* @brief Compute the 1d contact Jacobian and drift
94			*
95			* @param[in] data 1d 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 1d contact holonomic constraint
104			*
105			* @param[in] data 1d 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 1d contact data
116			* @param[in] force 1d force
117			*/
118			virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
119			const VectorXs& force) override;
120
121			/**
122			* @brief Create the 1d contact data
123			*/
124			virtual std::shared_ptr<ContactDataAbstract> createData(
125			pinocchio::DataTpl<Scalar>* const data) override;
126
127			/**
128			* @brief Cast the contact-1d 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 ContactModel1DTpl<NewScalar> A contact model with the
134			* new scalar type.
135			*/
136			template <typename NewScalar>
137			ContactModel1DTpl<NewScalar> cast() const;
138
139			/**
140			* @brief Return the reference frame translation
141			*/
142			const Scalar get_reference() const;
143
144			/**
145			* @brief Create the 1d contact data
146			*/
147			const Vector2s& get_gains() const;
148
149			/**
150			* @brief Return the rotation of the reference frames's z axis
151			*/
152			const Matrix3s& get_axis_rotation() const;
153
154			/**
155			* @brief Modify the reference frame translation
156			*/
157			void set_reference(const Scalar reference);
158
159			/**
160			* @brief Modify the rotation of the reference frames's z axis
161			*/
162			void set_axis_rotation(const Matrix3s& rotation);
163
164			/**
165			* @brief Print relevant information of the 1d contact model
166			*
167			* @param[out] os Output stream object
168			*/
169			virtual void print(std::ostream& os) const override;
170
171			protected:
172			using Base::id_;
173			using Base::nc_;
174			using Base::nu_;
175			using Base::state_;
176			using Base::type_;
177
178			private:
179			Scalar xref_; //!< Contact position used for the Baumgarte stabilization
180			Matrix3s Raxis_; //!< Rotation of the reference frame's z-axis
181			Vector2s gains_; //!< Baumgarte stabilization gains
182			};
183
184			template <typename _Scalar>
185			struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
186			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
187
188			typedef _Scalar Scalar;
189			typedef MathBaseTpl<Scalar> MathBase;
190			typedef ContactDataAbstractTpl<Scalar> Base;
191			typedef typename MathBase::Matrix2s Matrix2s;
192			typedef typename MathBase::Matrix3s Matrix3s;
193			typedef typename MathBase::Matrix3xs Matrix3xs;
194			typedef typename MathBase::Matrix6xs Matrix6xs;
195			typedef typename MathBase::Vector3s Vector3s;
196			typedef typename pinocchio::MotionTpl<Scalar> Motion;
197			typedef typename pinocchio::ForceTpl<Scalar> Force;
198
199			template <template <typename Scalar> class Model>
200		✗	ContactData1DTpl(Model<Scalar>* const model,
201			pinocchio::DataTpl<Scalar>* const data)
202			: Base(model, data),
203		✗	v(Motion::Zero()),
204		✗	f_local(Force::Zero()),
205		✗	da0_local_dx(3, model->get_state()->get_ndx()),
206		✗	fJf(6, model->get_state()->get_nv()),
207		✗	v_partial_dq(6, model->get_state()->get_nv()),
208		✗	a_partial_dq(6, model->get_state()->get_nv()),
209		✗	a_partial_dv(6, model->get_state()->get_nv()),
210		✗	a_partial_da(6, model->get_state()->get_nv()),
211		✗	fXjdv_dq(6, model->get_state()->get_nv()),
212		✗	fXjda_dq(6, model->get_state()->get_nv()),
213		✗	fXjda_dv(6, model->get_state()->get_nv()),
214		✗	fJf_df(3, model->get_state()->get_nv()) {
215		✗	frame = model->get_id();
216		✗	jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
217		✗	fXj = jMf.inverse().toActionMatrix();
218		✗	a0_local.setZero();
219		✗	dp.setZero();
220		✗	dp_local.setZero();
221		✗	da0_local_dx.setZero();
222		✗	fJf.setZero();
223		✗	v_partial_dq.setZero();
224		✗	a_partial_dq.setZero();
225		✗	a_partial_dv.setZero();
226		✗	a_partial_da.setZero();
227		✗	vv_skew.setZero();
228		✗	vw_skew.setZero();
229		✗	a0_skew.setZero();
230		✗	a0_world_skew.setZero();
231		✗	dp_skew.setZero();
232		✗	f_skew.setZero();
233		✗	fXjdv_dq.setZero();
234		✗	fXjda_dq.setZero();
235		✗	fXjda_dv.setZero();
236		✗	oRf.setZero();
237		✗	fJf_df.setZero();
238		✗	}
239		✗	virtual ~ContactData1DTpl() = default;
240
241			using Base::a0;
242			using Base::da0_dx;
243			using Base::df_du;
244			using Base::df_dx;
245			using Base::f;
246			using Base::frame;
247			using Base::fXj;
248			using Base::Jc;
249			using Base::jMf;
250			using Base::pinocchio;
251
252			Motion v;
253			Vector3s a0_local;
254			Vector3s dp;
255			Vector3s dp_local;
256			Force f_local;
257			Matrix3xs da0_local_dx;
258			Matrix6xs fJf;
259			Matrix6xs v_partial_dq;
260			Matrix6xs a_partial_dq;
261			Matrix6xs a_partial_dv;
262			Matrix6xs a_partial_da;
263			Matrix3s vv_skew;
264			Matrix3s vw_skew;
265			Matrix3s a0_skew;
266			Matrix3s a0_world_skew;
267			Matrix3s dp_skew;
268			Matrix3s f_skew;
269			Matrix6xs fXjdv_dq;
270			Matrix6xs fXjda_dq;
271			Matrix6xs fXjda_dv;
272			Matrix2s oRf;
273			Matrix3xs fJf_df;
274			};
275
276			} // namespace crocoddyl
277
278			/* --- Details -------------------------------------------------------------- */
279			/* --- Details -------------------------------------------------------------- */
280			/* --- Details -------------------------------------------------------------- */
281			#include "crocoddyl/multibody/contacts/contact-1d.hxx"
282
283			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel1DTpl)
284			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData1DTpl)
285
286			#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_1D_HPP_
287