GCC Code Coverage Report

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