GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contact-base.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 13 0.0%
Branches: 0 36 0.0%
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