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_2D_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_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 ContactModel2DTpl : public ContactModelAbstractTpl<_Scalar> {
20			public:
21			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22		✗	CROCODDYL_DERIVED_CAST(ContactModelBase, ContactModel2DTpl)
23
24			typedef _Scalar Scalar;
25			typedef MathBaseTpl<Scalar> MathBase;
26			typedef ContactModelAbstractTpl<Scalar> Base;
27			typedef ContactData2DTpl<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 2d contact model
37			*
38			* @param[in] state State of the multibody system
39			* @param[in] id Reference frame id of the contact
40			* @param[in] xref Contact position used for the Baumgarte stabilization
41			* @param[in] nu Dimension of the control vector
42			* @param[in] gains Baumgarte stabilization gains
43			*/
44			ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
45			const pinocchio::FrameIndex id, const Vector2s& xref,
46			const std::size_t nu,
47			const Vector2s& gains = Vector2s::Zero());
48
49			/**
50			* @brief Initialize the 2d contact model
51			*
52			* The default `nu` is obtained from `StateAbstractTpl::get_nv()`.
53			*
54			* @param[in] state State of the multibody system
55			* @param[in] id Reference frame id of the contact
56			* @param[in] xref Contact position used for the Baumgarte stabilization
57			* @param[in] gains Baumgarte stabilization gains
58			*/
59			ContactModel2DTpl(std::shared_ptr<StateMultibody> state,
60			const pinocchio::FrameIndex id, const Vector2s& xref,
61			const Vector2s& gains = Vector2s::Zero());
62		✗	virtual ~ContactModel2DTpl() = default;
63
64			/**
65			* @brief Compute the 2d contact Jacobian and drift
66			*
67			* @param[in] data 2d contact data
68			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
69			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
70			*/
71			virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
72			const Eigen::Ref<const VectorXs>& x) override;
73
74			/**
75			* @brief Compute the derivatives of the 2d contact holonomic constraint
76			*
77			* @param[in] data 2d contact data
78			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
79			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
80			*/
81			virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
82			const Eigen::Ref<const VectorXs>& x) override;
83
84			/**
85			* @brief Convert the force into a stack of spatial forces
86			*
87			* @param[in] data 2d contact data
88			* @param[in] force 2d force
89			*/
90			virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
91			const VectorXs& force) override;
92
93			/**
94			* @brief Create the 2d contact data
95			*/
96			virtual std::shared_ptr<ContactDataAbstract> createData(
97			pinocchio::DataTpl<Scalar>* const data) override;
98
99			/**
100			* @brief Cast the contact-2d model to a different scalar type.
101			*
102			* It is useful for operations requiring different precision or scalar types.
103			*
104			* @tparam NewScalar The new scalar type to cast to.
105			* @return ContactModel2DTpl<NewScalar> A contact model with the
106			* new scalar type.
107			*/
108			template <typename NewScalar>
109			ContactModel2DTpl<NewScalar> cast() const;
110
111			/**
112			* @brief Return the reference frame translation
113			*/
114			const Vector2s& get_reference() const;
115
116			/**
117			* @brief Create the 2d contact data
118			*/
119			const Vector2s& get_gains() const;
120
121			/**
122			* @brief Modify the reference frame translation
123			*/
124			void set_reference(const Vector2s& reference);
125
126			/**
127			* @brief Print relevant information of the 2d contact model
128			*
129			* @param[out] os Output stream object
130			*/
131			virtual void print(std::ostream& os) const override;
132
133			protected:
134			using Base::id_;
135			using Base::nc_;
136			using Base::nu_;
137			using Base::state_;
138
139			private:
140			Vector2s xref_; //!< Contact position used for the Baumgarte stabilization
141			Vector2s gains_; //!< Baumgarte stabilization gains
142			};
143
144			template <typename _Scalar>
145			struct ContactData2DTpl : public ContactDataAbstractTpl<_Scalar> {
146			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
147
148			typedef _Scalar Scalar;
149			typedef MathBaseTpl<Scalar> MathBase;
150			typedef ContactDataAbstractTpl<Scalar> Base;
151			typedef typename MathBase::Matrix2s Matrix2s;
152			typedef typename MathBase::Matrix3s Matrix3s;
153			typedef typename MathBase::Matrix6xs Matrix6xs;
154			typedef typename MathBase::Vector3s Vector3s;
155
156			template <template <typename Scalar> class Model>
157		✗	ContactData2DTpl(Model<Scalar>* const model,
158			pinocchio::DataTpl<Scalar>* const data)
159			: Base(model, data),
160		✗	fJf(6, model->get_state()->get_nv()),
161		✗	v_partial_dq(6, model->get_state()->get_nv()),
162		✗	a_partial_dq(6, model->get_state()->get_nv()),
163		✗	a_partial_dv(6, model->get_state()->get_nv()),
164		✗	a_partial_da(6, model->get_state()->get_nv()),
165		✗	fXjdv_dq(6, model->get_state()->get_nv()),
166		✗	fXjda_dq(6, model->get_state()->get_nv()),
167		✗	fXjda_dv(6, model->get_state()->get_nv()) {
168		✗	frame = model->get_id();
169		✗	jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
170		✗	fXj = jMf.inverse().toActionMatrix();
171		✗	fJf.setZero();
172		✗	v_partial_dq.setZero();
173		✗	a_partial_dq.setZero();
174		✗	a_partial_dv.setZero();
175		✗	a_partial_da.setZero();
176		✗	fXjdv_dq.setZero();
177		✗	fXjda_dq.setZero();
178		✗	fXjda_dv.setZero();
179		✗	vv.setZero();
180		✗	vw.setZero();
181		✗	vv_skew.setZero();
182		✗	vw_skew.setZero();
183		✗	oRf.setZero();
184		✗	}
185		✗	virtual ~ContactData2DTpl() = default;
186
187			using Base::a0;
188			using Base::da0_dx;
189			using Base::df_du;
190			using Base::df_dx;
191			using Base::f;
192			using Base::frame;
193			using Base::fXj;
194			using Base::Jc;
195			using Base::jMf;
196			using Base::pinocchio;
197
198			pinocchio::MotionTpl<Scalar> v;
199			pinocchio::MotionTpl<Scalar> a;
200			Matrix6xs fJf;
201			Matrix6xs v_partial_dq;
202			Matrix6xs a_partial_dq;
203			Matrix6xs a_partial_dv;
204			Matrix6xs a_partial_da;
205			Matrix6xs fXjdv_dq;
206			Matrix6xs fXjda_dq;
207			Matrix6xs fXjda_dv;
208			Vector3s vv;
209			Vector3s vw;
210			Matrix3s vv_skew;
211			Matrix3s vw_skew;
212			Matrix2s oRf;
213			};
214
215			} // namespace crocoddyl
216
217			/* --- Details -------------------------------------------------------------- */
218			/* --- Details -------------------------------------------------------------- */
219			/* --- Details -------------------------------------------------------------- */
220			#include "crocoddyl/multibody/contacts/contact-2d.hxx"
221
222			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModel2DTpl)
223			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactData2DTpl)
224
225			#endif // CROCODDYL_MULTIBODY_CONTACTS_CONTACT_2D_HPP_
226