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_CONTACT_BASE_HPP_
11			#define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
12
13			#include "crocoddyl/core/utils/deprecate.hpp"
14			#include "crocoddyl/multibody/force-base.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16			#include "crocoddyl/multibody/states/multibody.hpp"
17
18			namespace crocoddyl {
19
20			class ContactModelBase {
21			public:
22		✗	virtual ~ContactModelBase() = default;
23
24		✗	CROCODDYL_BASE_CAST(ContactModelBase, ContactModelAbstractTpl)
25			};
26
27			template <typename _Scalar>
28			class ContactModelAbstractTpl : public ContactModelBase {
29			public:
30			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32			typedef _Scalar Scalar;
33			typedef MathBaseTpl<Scalar> MathBase;
34			typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
35			typedef StateMultibodyTpl<Scalar> StateMultibody;
36			typedef typename MathBase::VectorXs VectorXs;
37			typedef typename MathBase::MatrixXs MatrixXs;
38
39			/**
40			* @brief Initialize the contact abstraction
41			*
42			* @param[in] state State of the multibody system
43			* @param[in] type Type of contact
44			* @param[in] nc Dimension of the contact model
45			* @param[in] nu Dimension of the control vector
46			*/
47			ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
48			const pinocchio::ReferenceFrame type,
49			const std::size_t nc, const std::size_t nu);
50			ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
51			const pinocchio::ReferenceFrame type,
52			const std::size_t nc);
53
54			DEPRECATED(
55			"Use constructor that passes the type type of contact, this assumes is "
56			"pinocchio::LOCAL",
57			ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
58			const std::size_t nc, const std::size_t nu);)
59			DEPRECATED(
60			"Use constructor that passes the type type of contact, this assumes is "
61			"pinocchio::LOCAL",
62			ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
63			const std::size_t nc);)
64		✗	virtual ~ContactModelAbstractTpl() = default;
65
66			/**
67			* @brief Compute the contact Jacobian and acceleration drift
68			*
69			* @param[in] data Contact data
70			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
71			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
72			*/
73			virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
74			const Eigen::Ref<const VectorXs>& x) = 0;
75
76			/**
77			* @brief Compute the derivatives of the acceleration-based contact
78			*
79			* @param[in] data Contact data
80			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
81			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
82			*/
83			virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
84			const Eigen::Ref<const VectorXs>& x) = 0;
85
86			/**
87			* @brief Convert the force into a stack of spatial forces
88			*
89			* @param[in] data Contact data
90			* @param[in] force Contact force
91			*/
92			virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
93			const VectorXs& force) = 0;
94
95			/**
96			* @brief Convert the force into a stack of spatial forces
97			*
98			* @param[in] data Contact data
99			* @param[in] force Contact force
100			*/
101			void updateForceDiff(const std::shared_ptr<ContactDataAbstract>& data,
102			const MatrixXs& df_dx, const MatrixXs& df_du) const;
103
104			/**
105			* @brief Set the stack of spatial forces to zero
106			*
107			* @param[in] data Contact data
108			*/
109			void setZeroForce(const std::shared_ptr<ContactDataAbstract>& data) const;
110
111			/**
112			* @brief Set the stack of spatial forces Jacobians to zero
113			*
114			* @param[in] data Contact data
115			*/
116			void setZeroForceDiff(const std::shared_ptr<ContactDataAbstract>& data) const;
117
118			/**
119			* @brief Create the contact data
120			*/
121			virtual std::shared_ptr<ContactDataAbstract> createData(
122			pinocchio::DataTpl<Scalar>* const data);
123
124			/**
125			* @brief Return the state
126			*/
127			const std::shared_ptr<StateMultibody>& get_state() const;
128
129			/**
130			* @brief Return the dimension of the contact
131			*/
132			std::size_t get_nc() const;
133
134			/**
135			* @brief Return the dimension of the control vector
136			*/
137			std::size_t get_nu() const;
138
139			/**
140			* @brief Return the reference frame id
141			*/
142			pinocchio::FrameIndex get_id() const;
143
144			/**
145			* @brief Modify the reference frame id
146			*/
147			void set_id(const pinocchio::FrameIndex id);
148
149			/**
150			* @brief Modify the type of contact
151			*/
152			void set_type(const pinocchio::ReferenceFrame type);
153
154			/**
155			* @brief Return the type of contact
156			*/
157			pinocchio::ReferenceFrame get_type() const;
158
159			/**
160			* @brief Print information on the contact model
161			*/
162			template <class Scalar>
163			friend std::ostream& operator<<(std::ostream& os,
164			const ContactModelAbstractTpl<Scalar>& model);
165
166			/**
167			* @brief Print relevant information of the contact model
168			*
169			* @param[out] os Output stream object
170			*/
171			virtual void print(std::ostream& os) const;
172
173			protected:
174			std::shared_ptr<StateMultibody> state_;
175			std::size_t nc_;
176			std::size_t nu_;
177			pinocchio::FrameIndex id_; //!< Reference frame id of the contact
178			pinocchio::ReferenceFrame type_; //!< Type of contact
179		✗	ContactModelAbstractTpl() : state_(nullptr), nc_(0), nu_(0), id_(0) {};
180			};
181
182			template <typename _Scalar>
183			struct ContactDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
184			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
185
186			typedef _Scalar Scalar;
187			typedef MathBaseTpl<Scalar> MathBase;
188			typedef ForceDataAbstractTpl<Scalar> Base;
189			typedef typename MathBase::VectorXs VectorXs;
190			typedef typename MathBase::MatrixXs MatrixXs;
191			typedef typename pinocchio::SE3Tpl<Scalar> SE3;
192
193			template <template <typename Scalar> class Model>
194		✗	ContactDataAbstractTpl(Model<Scalar>* const model,
195			pinocchio::DataTpl<Scalar>* const data)
196			: Base(model, data),
197		✗	fXj(jMf.inverse().toActionMatrix()),
198		✗	a0(model->get_nc()),
199		✗	da0_dx(model->get_nc(), model->get_state()->get_ndx()),
200		✗	dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
201		✗	a0.setZero();
202		✗	da0_dx.setZero();
203		✗	dtau_dq.setZero();
204		✗	}
205		✗	virtual ~ContactDataAbstractTpl() = default;
206
207			using Base::df_du;
208			using Base::df_dx;
209			using Base::f;
210			using Base::frame;
211			using Base::Jc;
212			using Base::jMf;
213			using Base::pinocchio;
214
215			typename SE3::ActionMatrixType fXj;
216			VectorXs a0;
217			MatrixXs da0_dx;
218			MatrixXs dtau_dq;
219			};
220
221			} // namespace crocoddyl
222
223			/* --- Details -------------------------------------------------------------- */
224			/* --- Details -------------------------------------------------------------- */
225			/* --- Details -------------------------------------------------------------- */
226			#include "crocoddyl/multibody/contact-base.hxx"
227
228			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModelAbstractTpl)
229			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactDataAbstractTpl)
230
231			#endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
232