GCC Code Coverage Report

Directory: ./
File: unittest/geometry-model.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 109 0.0%
Branches: 0 652 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #include <iostream>
6
7 #include "pinocchio/multibody/model.hpp"
8 #include "pinocchio/multibody/data.hpp"
9
10 #include "pinocchio/multibody/geometry.hpp"
11 #include "pinocchio/parsers/urdf.hpp"
12
13 #include <vector>
14 #include <boost/test/unit_test.hpp>
15
16 using namespace pinocchio;
17
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20 ✗ BOOST_AUTO_TEST_CASE(manage_collision_pairs)
21 {
22 std::string filename =
23 PINOCCHIO_MODEL_DIR
24 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
25 ✗ std::vector<std::string> package_dirs;
26 ✗ std::string mesh_dir = PINOCCHIO_MODEL_DIR;
27 ✗ package_dirs.push_back(mesh_dir);
28
29 ✗ Model model;
30 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
31 ✗ GeometryModel geom_model;
32 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
33 ✗ geom_model.addAllCollisionPairs();
34
35 ✗ for (Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)geom_model.ngeoms; ++i)
36 {
37 ✗ for (Eigen::DenseIndex j = i + 1; j < (Eigen::DenseIndex)geom_model.ngeoms; ++j)
38 {
39 ✗ BOOST_CHECK(geom_model.collisionPairMapping(i, j) < (int)geom_model.collisionPairs.size());
40 ✗ BOOST_CHECK(geom_model.collisionPairMapping(j, i) < (int)geom_model.collisionPairs.size());
41 ✗ BOOST_CHECK(geom_model.collisionPairMapping(j, i) == geom_model.collisionPairMapping(i, j));
42
43 ✗ if (geom_model.collisionPairMapping(i, j) != -1)
44 {
45 ✗ const PairIndex pair_index = (PairIndex)geom_model.collisionPairMapping(i, j);
46 ✗ const CollisionPair & cp_ref = geom_model.collisionPairs[pair_index];
47 ✗ const CollisionPair cp((size_t)i, (size_t)j);
48 ✗ BOOST_CHECK(cp == cp_ref);
49 }
50 }
51 }
52
53 ✗ GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero(
54 ✗ (Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms));
55
56 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
57 {
58 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
59 ✗ collision_map((Eigen::DenseIndex)cp.first, (Eigen::DenseIndex)cp.second) = true;
60 }
61 ✗ GeometryModel::MatrixXb collision_map_lower = collision_map.transpose();
62
63 ✗ GeometryModel geom_model_copy, geom_model_copy_lower;
64 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs);
65 ✗ pinocchio::urdf::buildGeom(
66 model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs);
67 ✗ geom_model_copy.setCollisionPairs(collision_map);
68 ✗ geom_model_copy_lower.setCollisionPairs(collision_map_lower, false);
69
70 ✗ BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size());
71 ✗ BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size());
72 ✗ for (size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k)
73 {
74 ✗ BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k]));
75 ✗ BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k]));
76 }
77 ✗ for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
78 {
79 ✗ BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k]));
80 ✗ BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k]));
81 }
82
83 {
84 ✗ GeometryData geom_data(geom_model);
85 ✗ geom_data.activateAllCollisionPairs();
86
87 ✗ for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
88 ✗ BOOST_CHECK(geom_data.activeCollisionPairs[k]);
89 }
90
91 {
92 ✗ GeometryData geom_data(geom_model);
93 ✗ geom_data.deactivateAllCollisionPairs();
94
95 ✗ for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
96 ✗ BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
97 }
98
99 {
100 ✗ GeometryData geom_data(geom_model), geom_data_copy(geom_model),
101 ✗ geom_data_copy_lower(geom_model);
102 ✗ geom_data_copy.deactivateAllCollisionPairs();
103 ✗ geom_data_copy_lower.deactivateAllCollisionPairs();
104
105 ✗ GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero(
106 ✗ (Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms));
107 ✗ for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
108 {
109 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
110 ✗ collision_map((Eigen::DenseIndex)cp.first, (Eigen::DenseIndex)cp.second) =
111 ✗ geom_data.activeCollisionPairs[k];
112 }
113 ✗ GeometryData::MatrixXb collision_map_lower = collision_map.transpose();
114
115 ✗ geom_data_copy.setActiveCollisionPairs(geom_model, collision_map);
116 ✗ BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs);
117
118 ✗ geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
119 ✗ BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
120 }
121
122 // Test security margins
123 {
124 ✗ GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model);
125
126 ✗ const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones(
127 ✗ (Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms));
128 ✗ GeometryData::MatrixXs security_margin_map_upper(security_margin_map);
129 ✗ security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.);
130
131 ✗ geom_data_upper.setSecurityMargins(geom_model, security_margin_map, true, true);
132 ✗ for (size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k)
133 {
134 ✗ BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.);
135 ✗ BOOST_CHECK(
136 geom_data_upper.collisionRequests[k].security_margin
137 == geom_data_upper.collisionRequests[k].distance_upper_bound);
138 }
139
140 ✗ geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false);
141 ✗ for (size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k)
142 {
143 ✗ BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.);
144 }
145 }
146
147 // Test enableGeometryCollision
148 {
149 ✗ GeometryData geom_data(geom_model);
150 ✗ geom_data.deactivateAllCollisionPairs();
151 ✗ geom_data.setGeometryCollisionStatus(geom_model, 0, true);
152
153 ✗ for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
154 {
155 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
156 ✗ if (cp.first == 0 || cp.second == 0)
157 {
158 ✗ BOOST_CHECK(geom_data.activeCollisionPairs[k]);
159 }
160 else
161 {
162 ✗ BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
163 }
164 }
165 }
166
167 // Test disableGeometryCollision
168 {
169 ✗ GeometryData geom_data(geom_model);
170 ✗ geom_data.activateAllCollisionPairs();
171 ✗ geom_data.setGeometryCollisionStatus(geom_model, 0, false);
172
173 ✗ for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
174 {
175 ✗ const CollisionPair & cp = geom_model.collisionPairs[k];
176 ✗ if (cp.first == 0 || cp.second == 0)
177 {
178 ✗ BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
179 }
180 else
181 {
182 ✗ BOOST_CHECK(geom_data.activeCollisionPairs[k]);
183 }
184 }
185 }
186 }
187
188 ✗ BOOST_AUTO_TEST_CASE(test_clone)
189 {
190 std::string filename =
191 PINOCCHIO_MODEL_DIR
192 ✗ + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
193 ✗ std::vector<std::string> package_dirs;
194 ✗ std::string mesh_dir = PINOCCHIO_MODEL_DIR;
195 ✗ package_dirs.push_back(mesh_dir);
196
197 ✗ Model model;
198 ✗ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
199 ✗ GeometryModel geom_model;
200 ✗ pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
201 ✗ geom_model.addAllCollisionPairs();
202
203 ✗ geom_model.geometryObjects[0].geometry =
204 ✗ GeometryObject::CollisionGeometryPtr(new ::hpp::fcl::Sphere(0.5));
205 ✗ GeometryModel geom_model_clone = geom_model.clone();
206 ✗ GeometryModel geom_model_copy = geom_model;
207
208 ✗ BOOST_CHECK(geom_model_clone == geom_model);
209 ✗ BOOST_CHECK(geom_model_copy == geom_model);
210
211 ✗ static_cast<::hpp::fcl::Sphere *>(geom_model.geometryObjects[0].geometry.get())->radius = 1.;
212 ✗ BOOST_CHECK(geom_model_clone != geom_model);
213 ✗ BOOST_CHECK(geom_model_copy == geom_model);
214 }
215
216 BOOST_AUTO_TEST_SUITE_END()
217