GCC Code Coverage Report

Directory: ./
File: include/multicontact-api/scenario/contact-model.hpp
Date: 2025-04-05 01:06:26
Exec Total Coverage
Lines: 65 65 100.0%
Branches: 30 50 60.0%
Line Branch Exec Source
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3
4 #ifndef __multicontact_api_scenario_contact_model_planar_hpp__
5 #define __multicontact_api_scenario_contact_model_planar_hpp__
6
7 #include <Eigen/Dense>
8 #include <iostream>
9 #include <pinocchio/fwd.hpp>
10 #include <pinocchio/spatial/skew.hpp>
11
12 #include "multicontact-api/scenario/fwd.hpp"
13 #include "multicontact-api/serialization/archive.hpp"
14 #include "multicontact-api/serialization/eigen-matrix.hpp"
15
16 namespace multicontact_api {
17 namespace scenario {
18
19 template <typename _Scalar>
20 struct ContactModelTpl
21 : public serialization::Serializable<ContactModelTpl<_Scalar> > {
22 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23
24 typedef _Scalar Scalar;
25 typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
26 typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
27 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6X;
28
29 /// \brief Default constructor.
30 284 ContactModelTpl()
31 284 : m_mu(-1.),
32 284 m_contact_type(ContactType::CONTACT_UNDEFINED),
33 284 m_num_contact_points(1),
34
1/2
✓ Branch 2 taken 284 times.
✗ Branch 3 not taken.
 284 m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
35
36 /// \brief Constructor with friction
37 155 ContactModelTpl(const Scalar mu)
38 155 : m_mu(mu),
39 155 m_contact_type(ContactType::CONTACT_UNDEFINED),
40 155 m_num_contact_points(1),
41
1/2
✓ Branch 2 taken 155 times.
✗ Branch 3 not taken.
 155 m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
42
43 /// \brief Full constructor
44 19 ContactModelTpl(const Scalar mu, const ContactType contact_type)
45 19 : m_mu(mu),
46 19 m_contact_type(contact_type),
47 19 m_num_contact_points(1),
48
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 19 m_contact_points_positions(Matrix3X::Zero(3, 1)) {}
49
50 /// \brief Copy constructor
51 template <typename S2>
52 explicit ContactModelTpl(const ContactModelTpl<S2>& other)
53 : m_mu(other.mu),
54 m_contact_type(other.m_contact_type),
55 m_contact_points_positions(other.m_contact_points_positions),
56 m_num_contact_points(other.m_num_contact_points) {}
57
58 template <typename S2>
59 470 bool operator==(const ContactModelTpl<S2>& other) const {
60
2/2
✓ Branch 0 taken 460 times.
✓ Branch 1 taken 2 times.
 462 return m_mu == other.m_mu && m_contact_type == other.m_contact_type &&
61
4/4
✓ Branch 0 taken 462 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 459 times.
✓ Branch 3 taken 1 times.
 1391 m_num_contact_points == other.m_num_contact_points &&
62
2/2
✓ Branch 1 taken 458 times.
✓ Branch 2 taken 1 times.
 929 m_contact_points_positions == other.m_contact_points_positions;
63 }
64
65 template <typename S2>
66 5 bool operator!=(const ContactModelTpl<S2>& other) const {
67 5 return !(*this == other);
68 }
69
70 4 void disp(std::ostream& os) const {
71 4 os << "ContactType: " << m_contact_type << ", mu: " << m_mu << std::endl
72 4 << "Number of contact points: " << m_num_contact_points
73 4 << ", positions: " << std::endl
74 4 << m_contact_points_positions << std::endl;
75 4 }
76
77 template <typename S2>
78 4 friend std::ostream& operator<<(std::ostream& os,
79 const ContactModelTpl<S2>& cp) {
80 4 cp.disp(os);
81 4 return os;
82 }
83
84 18 size_t num_contact_points() const { return m_num_contact_points; }
85
86 8 void num_contact_points(const size_t num) {
87 8 m_num_contact_points = num;
88
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 m_contact_points_positions = Matrix3X::Zero(3, num);
89 8 }
90
91 38 Matrix3X contact_points_positions() const {
92 38 return m_contact_points_positions;
93 }
94
95 13 void contact_points_positions(const Matrix3X& positions) {
96 13 m_contact_points_positions = positions;
97 13 m_num_contact_points = positions.cols();
98 13 }
99
100 /**
101 * @brief generatorMatrix Return a 6x(num_contact_points*3) matrix
102 * containing the generator used to compute contact forces.
103 * @return
104 */
105 2 Matrix6X generatorMatrix() const {
106
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
107
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 2 times.
 13 for (size_t i = 0; i < m_num_contact_points; i++) {
108
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 gen.block(0, i * 3, 3, 3) = Matrix3::Identity();
109
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 gen.block(3, i * 3, 3, 3) =
110
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 22 pinocchio::skew(m_contact_points_positions.col(i));
111 }
112 2 return gen;
113 }
114
115 /// \brief Friction coefficient.
116 Scalar m_mu;
117 /// \brief ZMP radius.
118 ContactType m_contact_type;
119
120 private:
121 /// \brief The number of contact points used to model this contact
122 size_t m_num_contact_points;
123 /// \brief 3xnum_contact_points matrix defining the contact points positions
124 /// in the frame of the contact placement
125 Matrix3X m_contact_points_positions;
126
127 // Serialization of the class
128 friend class boost::serialization::access;
129
130 template <class Archive>
131 352 void save(Archive& ar, const unsigned int /*version*/) const {
132
1/2
✓ Branch 2 taken 176 times.
✗ Branch 3 not taken.
 352 ar& boost::serialization::make_nvp("mu", m_mu);
133
1/2
✓ Branch 2 taken 176 times.
✗ Branch 3 not taken.
 352 ar& boost::serialization::make_nvp("contact_type", m_contact_type);
134
1/2
✓ Branch 1 taken 176 times.
✗ Branch 2 not taken.
 352 ar& boost::serialization::make_nvp("num_contact_points",
135 352 m_num_contact_points);
136
1/2
✓ Branch 1 taken 176 times.
✗ Branch 2 not taken.
 352 ar& boost::serialization::make_nvp("contact_points_positions",
137 352 m_contact_points_positions);
138 352 }
139
140 template <class Archive>
141 352 void load(Archive& ar, const unsigned int /*version*/) {
142
1/2
✓ Branch 2 taken 176 times.
✗ Branch 3 not taken.
 352 ar >> boost::serialization::make_nvp("mu", m_mu);
143
1/2
✓ Branch 2 taken 176 times.
✗ Branch 3 not taken.
 352 ar >> boost::serialization::make_nvp("contact_type", m_contact_type);
144
1/2
✓ Branch 1 taken 176 times.
✗ Branch 2 not taken.
 352 ar >> boost::serialization::make_nvp("num_contact_points",
145 352 m_num_contact_points);
146
1/2
✓ Branch 1 taken 176 times.
✗ Branch 2 not taken.
 352 ar >> boost::serialization::make_nvp("contact_points_positions",
147 352 m_contact_points_positions);
148 352 }
149
150 704 BOOST_SERIALIZATION_SPLIT_MEMBER()
151 };
152 } // namespace scenario
153 } // namespace multicontact_api
154
155 MULTICONTACT_API_DEFINE_CLASS_TEMPLATE_VERSION(
156 typename Scalar, multicontact_api::scenario::ContactModelTpl<Scalar>)
157
158 #endif // ifndef __multicontact_api_scenario_contact_model_planar_hpp__
159