GCC Code Coverage Report

Directory: ./
File: unittest/factory/contact.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 116 131 88.5%
Branches: 83 145 57.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "contact.hpp"
10
11 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
12 #include "crocoddyl/multibody/contacts/contact-2d.hpp"
13 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
14 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
15
16 namespace crocoddyl {
17 namespace unittest {
18
19 const std::vector<ContactModelTypes::Type> ContactModelTypes::all(
20 ContactModelTypes::init_all());
21
22 50 std::ostream& operator<<(std::ostream& os,
23 const ContactModelTypes::Type& type) {
24
10/12
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 5 times.
✓ Branch 6 taken 5 times.
✓ Branch 7 taken 5 times.
✓ Branch 8 taken 5 times.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 50 switch (type) {
25 5 case ContactModelTypes::ContactModel1D_LOCAL:
26 5 os << "ContactModel1D_LOCAL";
27 5 break;
28 5 case ContactModelTypes::ContactModel1D_WORLD:
29 5 os << "ContactModel1D_WORLD";
30 5 break;
31 5 case ContactModelTypes::ContactModel1D_LWA:
32 5 os << "ContactModel1D_LWA";
33 5 break;
34 5 case ContactModelTypes::ContactModel2D:
35 5 os << "ContactModel2D";
36 5 break;
37 5 case ContactModelTypes::ContactModel3D_LOCAL:
38 5 os << "ContactModel3D_LOCAL";
39 5 break;
40 5 case ContactModelTypes::ContactModel3D_WORLD:
41 5 os << "ContactModel3D_WORLD";
42 5 break;
43 5 case ContactModelTypes::ContactModel3D_LWA:
44 5 os << "ContactModel3D_LWA";
45 5 break;
46 5 case ContactModelTypes::ContactModel6D_LOCAL:
47 5 os << "ContactModel6D_LOCAL";
48 5 break;
49 5 case ContactModelTypes::ContactModel6D_WORLD:
50 5 os << "ContactModel6D_WORLD";
51 5 break;
52 5 case ContactModelTypes::ContactModel6D_LWA:
53 5 os << "ContactModel6D_LWA";
54 5 break;
55 ✗ case ContactModelTypes::NbContactModelTypes:
56 ✗ os << "NbContactModelTypes";
57 ✗ break;
58 ✗ default:
59 ✗ os << "Unknown type";
60 ✗ break;
61 }
62 50 return os;
63 }
64
65 2003 ContactModelFactory::ContactModelFactory() {}
66 2003 ContactModelFactory::~ContactModelFactory() {}
67
68 2003 std::shared_ptr<crocoddyl::ContactModelAbstract> ContactModelFactory::create(
69 ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type,
70 Eigen::Vector2d gains, const std::string frame_name, std::size_t nu) const {
71
1/2
✓ Branch 1 taken 2003 times.
✗ Branch 2 not taken.
 2003 PinocchioModelFactory model_factory(model_type);
72 std::shared_ptr<crocoddyl::StateMultibody> state =
73
2/4
✓ Branch 1 taken 2003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2003 times.
✗ Branch 5 not taken.
 2003 std::make_shared<crocoddyl::StateMultibody>(model_factory.create());
74 2003 std::shared_ptr<crocoddyl::ContactModelAbstract> contact;
75 2003 std::size_t frame_id = 0;
76
2/2
✓ Branch 1 taken 339 times.
✓ Branch 2 taken 1664 times.
 2003 if (frame_name == "") {
77
1/2
✓ Branch 1 taken 339 times.
✗ Branch 2 not taken.
 339 frame_id = model_factory.get_frame_ids()[0];
78 } else {
79
2/4
✓ Branch 2 taken 1664 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1664 times.
✗ Branch 7 not taken.
 1664 frame_id = state->get_pinocchio()->getFrameId(frame_name);
80 }
81
2/2
✓ Branch 1 taken 339 times.
✓ Branch 2 taken 1664 times.
 2003 if (nu == std::numeric_limits<std::size_t>::max()) {
82
1/2
✓ Branch 2 taken 339 times.
✗ Branch 3 not taken.
 339 nu = state->get_nv();
83 }
84
10/11
✓ Branch 0 taken 42 times.
✓ Branch 1 taken 30 times.
✓ Branch 2 taken 30 times.
✓ Branch 3 taken 132 times.
✓ Branch 4 taken 685 times.
✓ Branch 5 taken 248 times.
✓ Branch 6 taken 248 times.
✓ Branch 7 taken 298 times.
✓ Branch 8 taken 260 times.
✓ Branch 9 taken 30 times.
✗ Branch 10 not taken.
 2003 switch (contact_type) {
85 42 case ContactModelTypes::ContactModel1D_LOCAL: {
86
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 pinocchio::SE3 M = pinocchio::SE3::Random();
87
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 gains[0] =
88 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
89 42 contact = std::make_shared<crocoddyl::ContactModel1D>(
90
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL, M.rotation(),
91 42 nu, gains);
92 42 break;
93 }
94 30 case ContactModelTypes::ContactModel1D_WORLD: {
95
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 pinocchio::SE3 M = pinocchio::SE3::Random();
96
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 gains[0] =
97 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
98 30 contact = std::make_shared<crocoddyl::ContactModel1D>(
99
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 state, frame_id, 0., pinocchio::ReferenceFrame::WORLD, M.rotation(),
100 30 nu, gains);
101 30 break;
102 }
103 30 case ContactModelTypes::ContactModel1D_LWA: {
104
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 pinocchio::SE3 M = pinocchio::SE3::Random();
105
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 gains[0] =
106 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
107 30 contact = std::make_shared<crocoddyl::ContactModel1D>(
108 ✗ state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
109
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 M.rotation(), nu, gains);
110 30 break;
111 }
112 132 case ContactModelTypes::ContactModel2D:
113
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 132 gains[0] =
114 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
115
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 264 contact = std::make_shared<crocoddyl::ContactModel2D>(
116
1/2
✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
 264 state, frame_id, Eigen::Vector2d::Zero(), nu, gains);
117 132 break;
118 685 case ContactModelTypes::ContactModel3D_LOCAL:
119
1/2
✓ Branch 1 taken 685 times.
✗ Branch 2 not taken.
 1370 contact = std::make_shared<crocoddyl::ContactModel3D>(
120 ✗ state, frame_id, Eigen::Vector3d::Zero(),
121
1/2
✓ Branch 1 taken 685 times.
✗ Branch 2 not taken.
 1370 pinocchio::ReferenceFrame::LOCAL, nu, gains);
122 685 break;
123 248 case ContactModelTypes::ContactModel3D_WORLD:
124
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 496 contact = std::make_shared<crocoddyl::ContactModel3D>(
125 ✗ state, frame_id, Eigen::Vector3d::Zero(),
126
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 496 pinocchio::ReferenceFrame::WORLD, nu, gains);
127 248 break;
128 248 case ContactModelTypes::ContactModel3D_LWA:
129
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 496 contact = std::make_shared<crocoddyl::ContactModel3D>(
130 ✗ state, frame_id, Eigen::Vector3d::Zero(),
131
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 496 pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
132 248 break;
133 298 case ContactModelTypes::ContactModel6D_LOCAL:
134
1/2
✓ Branch 1 taken 298 times.
✗ Branch 2 not taken.
 596 contact = std::make_shared<crocoddyl::ContactModel6D>(
135 ✗ state, frame_id, pinocchio::SE3::Identity(),
136
1/2
✓ Branch 1 taken 298 times.
✗ Branch 2 not taken.
 596 pinocchio::ReferenceFrame::LOCAL, nu, gains);
137 298 break;
138 260 case ContactModelTypes::ContactModel6D_WORLD:
139
1/2
✓ Branch 1 taken 260 times.
✗ Branch 2 not taken.
 520 contact = std::make_shared<crocoddyl::ContactModel6D>(
140 ✗ state, frame_id, pinocchio::SE3::Identity(),
141
1/2
✓ Branch 1 taken 260 times.
✗ Branch 2 not taken.
 520 pinocchio::ReferenceFrame::WORLD, nu, gains);
142 260 break;
143 30 case ContactModelTypes::ContactModel6D_LWA:
144
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 60 contact = std::make_shared<crocoddyl::ContactModel6D>(
145 ✗ state, frame_id, pinocchio::SE3::Identity(),
146
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 60 pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
147 30 break;
148 ✗ default:
149 ✗ throw_pretty(__FILE__ ": Wrong ContactModelTypes::Type given");
150 break;
151 }
152 4006 return contact;
153 2003 }
154
155 39 std::shared_ptr<crocoddyl::ContactModelAbstract> create_random_contact() {
156 static bool once = true;
157
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 38 times.
 39 if (once) {
158 1 srand((unsigned)time(NULL));
159 1 once = false;
160 }
161 39 std::shared_ptr<crocoddyl::ContactModelAbstract> contact;
162
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 ContactModelFactory factory;
163
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 27 times.
 39 if (rand() % 4 == 0) {
164
3/6
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
 48 contact = factory.create(ContactModelTypes::ContactModel1D_LOCAL,
165 PinocchioModelTypes::RandomHumanoid,
166
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 36 Eigen::Vector2d::Random());
167
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 25 times.
 27 } else if (rand() % 4 == 1) {
168
3/6
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 8 contact = factory.create(ContactModelTypes::ContactModel2D,
169 PinocchioModelTypes::RandomHumanoid,
170
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 6 Eigen::Vector2d::Random());
171
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 18 times.
 25 } else if (rand() % 4 == 2) {
172
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 28 contact = factory.create(ContactModelTypes::ContactModel3D_LOCAL,
173 PinocchioModelTypes::RandomHumanoid,
174
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 21 Eigen::Vector2d::Random());
175 } else {
176
3/6
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
 72 contact = factory.create(ContactModelTypes::ContactModel6D_LOCAL,
177 PinocchioModelTypes::RandomHumanoid,
178
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 54 Eigen::Vector2d::Random());
179 }
180 78 return contact;
181 39 }
182
183 } // namespace unittest
184 } // namespace crocoddyl
185